
import lejos.nxt.*;

public class OrangeController {
	//DEFAULTS, CONSTANTS & SETTINGS
	boolean VERBOSE = true;
	double DEF_CIRC = 17.19;
	double DEF_AXIS = 15.09;
	int TXT_POSX = 1;
	int TXT_POSY = LCD.DISPLAY_CHAR_DEPTH-2;
	int DEF_TURN_SPEED = 400;
	int DEF_DRIVE_SPEED = 500;

	//fields
	private double circ; //circumference, measure by Motor.X.rotate(360), in cm
	private double axis; //distance between the centers of the 2 wheels
	private double turn; // turning circle around the center
	private int spd_turn;
	private int spd_drive;

	/**
	*Constructor
	* wheel_circ : wheel circumference in centimeters
	* axis_width : axis width in centimeters
	* turn_speed : rotational speed during turning
	* drive_speed : rotational speed during driving
	*/
	public OrangeController(double wheel_circ, double axis_width, int turn_speed, int drive_speed) {
		circ = wheel_circ;
		axis = axis_width;
		spd_turn = turn_speed;
		spd_drive = drive_speed;
		turn = axis*Math.PI;
	}
	
	/**
	*Constructor
	* using default values for circumference and axis
	*/
	public OrangeController() {
		circ = DEF_CIRC;
		axis = DEF_AXIS;
		spd_turn = DEF_TURN_SPEED;
		spd_drive = DEF_DRIVE_SPEED;
		turn = axis*Math.PI;
	}
	
	/**
	*Drive forward
	* dist : distance in cm
	*/
	public void forward(double dist) {
		Motor.C.setSpeed(spd_drive);
		Motor.B.setSpeed(spd_drive);	
		int rot = (int)((dist/circ)*360);
		if(VERBOSE) LCD.drawString("Forward",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rot,true);
		Motor.C.rotate(rot);
		if(VERBOSE) LCD.drawString("       ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Drive backward
	* dist : distance in cm
	*/
	public void backward(double dist) {
		Motor.C.setSpeed(spd_drive);
		Motor.B.setSpeed(spd_drive);
		int rot = (int)((dist/circ)*-360);
		if(VERBOSE) LCD.drawString("Backward",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rot,true);
		Motor.C.rotate(rot);
		if(VERBOSE) LCD.drawString("         ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Turn CCW
	* angle : angle in degrees
	*/
	public void turnCCW(double ang) {
		Motor.C.setSpeed(spd_turn);
		Motor.B.setSpeed(spd_turn);
		int rot = (int)((ang*turn)/circ);
		if(VERBOSE) LCD.drawString("Turn CCW",TXT_POSX,TXT_POSY);
		Motor.B.rotate(-rot,true);
		Motor.C.rotate(rot);
		if(VERBOSE) LCD.drawString("        ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Turn CW
	* angle : angle in degrees
	*/
	public void turnCW(double ang) {
		Motor.C.setSpeed(spd_turn);
		Motor.B.setSpeed(spd_turn);
		int rot = (int)((ang*turn)/circ);
		if(VERBOSE) LCD.drawString("Turn CW",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rot,true);
		Motor.C.rotate(-rot);
		if(VERBOSE) LCD.drawString("       ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Left Turn CCW
	* angle : angle in degrees around the left wheel
	*/
	public void turnLCCW(double ang) {
		Motor.C.setSpeed(spd_turn);
		Motor.B.setSpeed(spd_turn);
		int rot = (int)((ang*turn*2)/circ);
		if(VERBOSE) LCD.drawString("LTurn CCW",TXT_POSX,TXT_POSY);
		Motor.C.rotate(rot);
		if(VERBOSE) LCD.drawString("         ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Left Turn CW
	* angle : angle in degrees around the left wheel
	*/
	public void turnLCW(double ang) {
		Motor.C.setSpeed(spd_turn);
		Motor.B.setSpeed(spd_turn);
		int rot = (int)((ang*turn*2)/circ);
		if(VERBOSE) LCD.drawString("LTurn CW",TXT_POSX,TXT_POSY);
		Motor.C.rotate(-rot);
		if(VERBOSE) LCD.drawString("        ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Right Turn CCW
	* angle : angle in degrees around the left wheel
	*/
	public void turnRCCW(double ang) {
		Motor.C.setSpeed(spd_turn);
		Motor.B.setSpeed(spd_turn);
		int rot = (int)((ang*turn*2)/circ);
		if(VERBOSE) LCD.drawString("RTurn CCW",TXT_POSX,TXT_POSY);
		Motor.B.rotate(-rot);
		if(VERBOSE) LCD.drawString("         ",TXT_POSX,TXT_POSY);
	}
	
	/**
	*Right Turn CW
	* angle : angle in degrees around the left wheel
	*/
	public void turnRCW(double ang) {
		Motor.C.setSpeed(spd_turn);
		Motor.B.setSpeed(spd_turn);
		int rot = (int)((ang*turn*2)/circ);
		if(VERBOSE) LCD.drawString("RTurn CW",TXT_POSX,TXT_POSY);
		Motor.B.rotate(rot);
		if(VERBOSE) LCD.drawString("        ",TXT_POSX,TXT_POSY);
	}
	
	public double getAxis() { return axis; }
	public double getCirc() { return circ; }
}